#include <chrono>
#include <thread>
#include <iostream>
#include <csignal>
#include <unistd.h>
#include "motor_drivers.hpp"
#include "motor_sockets/can_socket.hpp"


int main(){
    MotorDrivers::CANSocket m3508_s1("can0", 0x201, 0x200, 8);
    auto m1 = std::make_shared<MotorDrivers::MotorM3508>(&m3508_s1);
    m1.get()->motor_SetMode(0);
    m1.get()->motor_SetTargetPos(10.0);
    // m1.get()->pid_pos.kp = 4.9;
    // m1.get()->pid_pos.ki = 0.02;
    // m1.get()->pid_pos.kd = 0.3;

    m1.get()->motor_SetTargetVel(5.0);
    // m1.get()->pid_speed.kp = 1.0;
    // m1.get()->pid_speed.ki = 0.1;
    // m1.get()->pid_speed.kd = 0.1;



    MotorDrivers::CANSocket c620_s1("can0", 0x200, 0x200, 8);
    MotorDrivers::CANSocket c620_s2("can0", 0x200, 0x1FF, 8);
    std::vector<std::shared_ptr<MotorDrivers::MotorM3508>> motors;
    motors.push_back(m1);

    MotorDrivers::M3508_Matrix m3508(&motors, &c620_s1, &c620_s2);
    m3508.motor_Init();
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    while(true){
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        m3508.motor_Update();
    }

    return 0;
}